#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Sensor, S2,     HTPB,                sensorI2CCustom9V)
#pragma config(Sensor, S3,     SMUX_1,              sensorI2CCustom)
#pragma config(Sensor, S4,     SMUX_2,              sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     fr,            tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     br,            tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     fl,            tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     bl,            tmotorNormal, PIDControl, reversed, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#ifndef COMPLETEDRIVER
#include "drivers/CompleteDriver.h"
#endif

task main{
  wait1Msec(50);    // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
  StartTask(sensorRead);
  bDisplayDiagnostics = false;
  kalmindebugdisplay = true;
  eraseDisplay();
  startkalmin();
  wait1Msec(50);
  StartTask(Jdrive);
  wait1Msec(1000);
  MoveDAS(360-45,50000,40);
}
